
#include <string.h>
#include "pid.h"


/*
    bsp_pid使用方法：
        首先在头文件中选择要使用什么类型的pid
        接着调用pid_xxx_init()函数，将pid参数写入其中
        然后调用pid_xxx_calculate()函数，输入目标量和当前量
        即可计算出结果

        位置式输出的是直接的结果，与上次的输出结果没关系
        增量式输出的是与上次结果的差值，需要在上次的输出结果上修改
*/


/**
 * @brief 增量式PID初始化
 * @param  pid: pid结构体
 * @param  kp: 比例系数
 * @param  ki: 积分系数
 * @param  kd: 微分系数
*/
int pid_inc_init (pid_inc_t *pid, float kp, float ki, float kd)
{
    memset(pid, 0, sizeof(pid_inc_t));

    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;

    return 0;
}


/**
 * @brief  位置式PID初始化
 * @param  pid: pid结构体
 * @param  kp: 比例系数
 * @param  ki: 积分系数
 * @param  kd: 微分系数
 * @param  sum_max: 积分限幅
 * @param  sum_min: 积分限下限(负数)
*/
int pid_pos_init (pid_pos_t *pid, float kp, float ki, float kd, float sum_max, float sum_min)
{
    memset(pid, 0, sizeof(pid_pos_t));

    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->sum_max = sum_max;
    pid->sum_min = sum_min;
    return 0;
}


/**
 * @brief  pid增量式计算
 * @param  pid: pid结构体
 * @param  SetValue: 目标值
 * @param  ActualValue: 当前值
 * @retval 计算结果
*/
float pid_inc_calculate (pid_inc_t *pid, float SetValue, float ActualValue)
{
    float ret;
    pid->cur_err = SetValue - ActualValue;
    #ifdef INC_EAZY
        /*
        增量公式为 Δu=Kp*{e(n)-e(n-1)}+Ki*e(n)+Kd*{e(n)-2*e(n-1)+e(n-2)}
        整理为 Δu=（Kp+Ki+Kd)*e(n)-(Kp+2Kd)*e(n-1)+Kd*e(n-2)
        将前面的汇集成一个参数，为 Δu=KP*e(n)-KI*e(n-1)+KD*e(n-2)
        */
        ret = pid->Kp*pid->cur_err - pid->Ki*pid->pre_err + pid->Kd*pid->pre2_err;
    #else
        ret = pid->Kp * (pid->cur_err - pid->pre_err)
            + pid->Ki * pid->cur_err
            + pid->Kd * (pid->cur_err - 2*pid->pre_err + pid->pre2_err);
    #endif
    pid->pre2_err = pid->pre_err;
    pid->pre_err = pid->cur_err;

    return ret;
}


/**
 * @brief  pid位置式计算
 * @param  pid: pid结构体
 * @param  SetValue: 目标值
 * @param  ActualValue: 当前值
 * @retval 计算结果
*/
float pid_pos_calculate (pid_pos_t *pid, float SetValue, float ActualValue)
{
    float output;
    /* 误差计算 */
    pid->cur_err = SetValue - ActualValue;
    pid->locSum += pid->cur_err;
    /* 积分限幅 */
    pid->locSum = pid->locSum > pid->sum_max ? pid->sum_max : pid->locSum;
    pid->locSum = pid->locSum < pid->sum_min ? pid->sum_min : pid->locSum;
    /* 位置式输出 */
    output = pid->Kp*pid->cur_err + pid->Ki*pid->locSum + pid->Kd*(pid->cur_err-pid->pre_err);
    pid->pre_err = pid->cur_err;

    return output;
}

